import pygame
from robomaster import robot
from MyTello import MyTello

def joyStick(tello):
    flight=tello.flight
    # battery=tello.battery


    # cam=tello.camera
    pygame.init()
    # Set the width and height of the screen [width,height]
    size = [500, 700]
    screen = pygame.display.set_mode(size)
    pygame.display.set_caption("tello controller")
    # Loop until the user clicks the close button.
    done = False
    # Used to manage how fast the screen updates
    clock = pygame.time.Clock()
    # Initialize the joysticks
    pygame.joystick.init()
    deadZone=15
    takeOffStates=False
    buttonState='down'
    pitch = 0.0
    roll = 0.0
    throttle = 0.0
    yaw = 0.0
    takeOff=0
    PITCH = 1
    ROLL = 0
    THROTTLE = 3
    YAW = 2
    TAKEOff=4
    pitchO = 0.0
    rollO = 0.0
    throttleO = 0.0
    yawO = 0.0
    for i in range(5):
        for event in pygame.event.get():  # User did something
            if event.type == pygame.QUIT:  # If user clicked close
                done = True  # Flag that we are done so we exit this loop

        js = pygame.joystick.Joystick(0)
        js.init()
        js_name = js.get_name()
        print('Joystick name: ' + js_name)
        if js_name == 'SAILI Simulator --- G3.5 Controller':
            axes = js.get_numaxes()
            
            # buttons = JoystickFlySky
            print('start get center value')
            pitchO = js.get_axis(PITCH)
            rollO = js.get_axis(ROLL)
            throttleO = js.get_axis(THROTTLE)
            yawO = js.get_axis(YAW)
            print('Joystick center are: pitch'+str(PITCH)+'='+str(pitchO)+',rol='+str(rollO)+',throttle=' + str(throttleO)+',yaw='+str(yawO)+', axis count='+str(pygame.joystick.get_count()))
            

    camOpened=False
    # Get ready to print

    # -------- Main Program Loop -----------
    while done == False:

        for event in pygame.event.get():  # User did something
            if event.type == pygame.QUIT:  # If user clicked close
                done = True  # Flag that we are done so we exit this loop

            # Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION
            if event.type == pygame.JOYBUTTONDOWN:
                # print("Joystick button pressed.")
                buttonState='down'
            if event.type == pygame.JOYBUTTONUP:
                # print("Joystick button released.")
                buttonState='up'


        # Get count of joysticks
        joystick_count = pygame.joystick.get_count()

        # For each joystick:
        for i in range(joystick_count):
            joystick = pygame.joystick.Joystick(i)
            joystick.init()


            # Get the name from the OS for the controller/joystick
            name = joystick.get_name()


            # Usually axis run in pairs, up/down for one, and left/right for
            # the other.
            axes = joystick.get_numaxes()


            for i in range(axes):
                if i == PITCH:
                    pitch = joystick.get_axis(PITCH)-pitchO
                    pitch=round(pitch*100)
                elif i == ROLL:
                    roll = joystick.get_axis(ROLL)-rollO
                    roll=round(roll*100)
                elif i == THROTTLE:
                    throttle = joystick.get_axis(THROTTLE)-throttleO
                    throttle=round(throttle*100)
                elif i == YAW:
                    yaw = joystick.get_axis(YAW)-yawO
                    yaw=round(yaw*100)
                else:
                    # axis = joystick.get_axis( i )
                    pass
                # textPrint.print(screen, "Axis {} value: {:>6.3f}".format(i, axis))
                # textPrint.indent()
            if abs(roll)<deadZone :
                roll=0
            if abs(pitch)<deadZone:
                pitch=0
            if abs(yaw)<deadZone:
                yaw=0
            if abs(throttle)<deadZone:
                throttle=0
            flight.rc(a=roll, b=pitch, c=throttle, d=yaw)

            print("pitch:{} ,roll:{} ,throttle:{} ,yaw:{}".format(pitch, roll, throttle, yaw))


            buttons = joystick.get_numbuttons()


            for i in range(buttons):
                if i==TAKEOff:
                    takeOff=joystick.get_button(TAKEOff)
                # button = joystick.get_button(i)
                # textPrint.print(screen, "Button {:>2} value: {}".format(i, button))
            print("takeoff is:{}".format(takeOff))

            if buttonState=='up':
                if  takeOffStates==False:
                    flight.takeoff()#.wait_for_completed()
                    takeOffStates=True
                    print('take off')
            if buttonState=='down':
                if takeOffStates:
                    flight.land()#.wait_for_completed()
                    takeOffStates=False
                    print('land')
                    
            # acc=tello.get_acceleration()
            # textPrint.print(screen,"acceleration:{}".format(acc))
            # att=tello.get_attitude()
            # textPrint.print(screen,"attitude:{}".format(att))
            # height=tello.get_height()
            # textPrint.print(screen,"height:{}".format(height))
            
            # batt=battery.get_battery()
            # textPrint.print(screen,"battery:{}".format(batt))

if __name__=='__main__':
    tello=MyTello()
    joyStick(tello)